/*
 * test.cpp
 *
 *  Created on: Mar 6, 2016
 *      Author: shawn
 */

#include "KCFCpp/kcftracker.hpp"
#include "uart.hpp"
#include "encoder.hpp"
#include <stdlib.h>
#include <stdio.h>
#include <time.h>
#include <opencv2/opencv.hpp>
#include <iostream>
#include <string>
#include <math.h>

//#include <opencv2/core/core.hpp>
//#include <opencv2/highgui/highgui.hpp>
//#include <opencv2/imgproc/imgproc.hpp>

//#define MIN(x,y) (x>y?y:x)

#define GET_CODEC_FROM_FILE

using namespace std;
using namespace cv;

Rect select_region;
bool selected_flag = false;
bool region_ready = false;
Point origin;
Mat frame, show_frame, show_frame2, roi_frame;
Point2f target1,target2;
vector<Point2f> Center_set;
void onMouse(int event, int x, int y, int, void*)
{
    if(selected_flag)
    {
        select_region.x = MIN(origin.x, x);
        select_region.y = MIN(origin.y, y);
        select_region.width = abs(x - origin.x);
        select_region.height = abs(y - origin.y);
        select_region &= Rect(0, 0, frame.cols, frame.rows);

        show_frame2 = show_frame.clone();
        rectangle(show_frame2, select_region, Scalar(255, 0, 0), 1, 8, 0);
    }
    if(event == CV_EVENT_LBUTTONDOWN)
    {
        selected_flag = true;
        origin = Point(x, y);
        select_region = Rect(x, y, 0, 0);
    }
    else if(event == CV_EVENT_LBUTTONUP)
    {
        region_ready = true;
        selected_flag = false;
    }

}

/*For 640*480*/
void ImgCoor2WorldCoor(const Point2f& imgCoor, Point2f& worldCoor)
{
	const double zc=1938.9120;
	worldCoor.x=0.004484720515239*(zc*imgCoor.x-76121.94074862002)-0.000056483151001*(zc*imgCoor.y-1276.469536789984);
	worldCoor.y=-0.000091272211797*(zc*imgCoor.x-76121.94074862002)+0.004428193220735*(zc*imgCoor.y-1276.469536789984);
}

/*For 1292*964*/
void WorldCoor2ImgCoor(Point2f& imgCoor, const Point2f& worldCoor)
{
	const double zc=1938.9120;
	imgCoor.x = (454.4656289800606*worldCoor.x + 5.713148560085065 * worldCoor.y + 145089.5663010601)/zc;
	imgCoor.y = (9.303905740019113*worldCoor.x + 458.2503948700810 * worldCoor.y - 1981.4181180401)/zc;
}
void ImgCoor2WorldCoor2(const Point2f& imgCoor, Point2f& worldCoor)
{
	const double zc=1938.9120;
	worldCoor.x= 0.002200948178338*(zc*imgCoor.x-145089.5663010601)-0.000027439897612*(zc*imgCoor.y+1981.4181180401);
	worldCoor.y= -0.000044686081276*(zc*imgCoor.x-145089.5663010601)+0.002182770182892*(zc*imgCoor.y+1981.4181180401);
}
//#define GET_CODEC_FROM_FILE

int main(int argc, char* argv[]) {
    bool HOG = true;
    bool FIXEDWINDOW = false;
    bool MULTISCALE = true;
    //bool SILENT = true;
    bool LAB = false;

    KCFTracker tracker(HOG, FIXEDWINDOW, MULTISCALE, LAB);
    //Uart uart("ttyACM0", 9600);
    PosEncoder encoder;

    VideoCapture fileVideo;
//    int frameToStart = 5000;
//    fileVideo.set( CV_CAP_PROP_POS_FRAMES,frameToStart);
    //   string strFilePath = "./cap2.avi";
    string strFilePath = argv[1];
    fileVideo.open(strFilePath);
    if (!fileVideo.isOpened()) {
        cout << "failed to open the video file: " << strFilePath << endl;
        return -1;
    }

    namedWindow("camera", 1);
    //double rate = capture.get(CV_CAP_PROP_FPS);
    //int delay = 1000/rate;
    setMouseCallback("camera", onMouse, 0);
    int real_roi_start_x = 622;
    int real_roi_start_y = 673;
    int real_roi_width = 130;
    int real_roi_height = 130;
    int roi_center_x = real_roi_start_x+real_roi_width/2;
    int roi_center_y = real_roi_start_y+real_roi_height/2;
    int roi_width = 200;
    int roi_height = 200;
    int roi_start_x = roi_center_x-roi_width/2;
    int roi_start_y = roi_center_y-roi_height/2;

    Size video_size = Size((int) fileVideo.get(CV_CAP_PROP_FRAME_WIDTH),
                           (int) fileVideo.get(CV_CAP_PROP_FRAME_HEIGHT));
    VideoWriter output_video;
    if(argv[2]) {
        char EXT[] = "MJPG";
        int ex = EXT[0] | (EXT[1] << 8) | (EXT[2] << 16) | (EXT[3] << 24);
        output_video.open(argv[2], ex, 25, Size(roi_width,roi_height), true);
        if (!output_video.isOpened()) {
            cout << "Can not open the output video for write" << endl;
            return -1;
        }
    }
    char c;

    bool tracker_ready = false;
    //bool last_selected_flag = selected_flag;
    Rect tracker_result;

    //Mat show_frame;

    clock_t start = clock();
    clock_t t_clock = start;
    Mat Draw = Mat::zeros(1292, 964, CV_8UC3);
    int i = 255;
    int j = 0;
    int k = 255;
    int frame_count = 0;
    int frame_count1 = 0;
    int frame_count2 = 0;
    bool record_flag = false;
    bool quit_flag = false;

    while(1)
    {
        fileVideo >> frame;
        frame_count++;
        frame.copyTo(show_frame);
        if(frame.empty())
        {
            fileVideo.release();
            return -1;
        }

        if(selected_flag == false && region_ready == true)
        {
            if(select_region.area() > 25)
            {
                tracker.init(select_region, frame);
                region_ready = false;
                tracker_ready = true;
            }
        }

        if(selected_flag)
        {
            rectangle(show_frame, select_region, Scalar(255, 0, 0), 1, 8, 0);
        }

        if(tracker_ready == true)
        {
            tracker_result = tracker.update(frame);
            rectangle(show_frame, tracker_result, Scalar(0, 255, 255),1,8,0);
        }

        int center_x = tracker_result.x + tracker_result.width / 2;
        int center_y = tracker_result.y + tracker_result.height /2;
        Point2f Center(center_x, center_y);
        int color_flag = 0;
        if( tracker_ready == true ){
            Center_set.push_back(Center);
            k=255;
            j=255;
            for(size_t q=0; q<Center_set.size(); q++){
                 circle(show_frame, Center_set[q], 1, CV_RGB(i,j,k), -1, 8, 0);
                if(!(color_flag%2)) {
                    if (k == 0) j--;
                    else k--;
                }
                color_flag++;
                }
        }


               //namedWindow("Draw",1);
        //imshow("Draw", Draw);
        /*
        char buf[20];
        sprintf(buf, "%d\t%d\n\r", tracker_result.x + tracker_result.width / 2,\
                                 tracker_result.y + tracker_result.height /2);
        uart.send(buf, strlen(buf));
        */

        static int count = 0;
        if(count > -1)
        {
            //std::cout << center_x << "\t" << center_y << std::endl;
            //uart.send(encoder.encode(center_x, center_y));
            count = 0;
        }
        count ++;
        roi_frame = show_frame(Rect(roi_start_x, roi_start_y, roi_width, roi_height));
        if(output_video.isOpened() && record_flag)
            output_video << roi_frame;
        imshow("camera", show_frame);
        imshow("ROI", roi_frame);
		Point2f world_coor;
		Point2f image_coor((tracker_result.x + tracker_result.width / 2.0f), (tracker_result.y + tracker_result.height / 2.0f));
//		ImgCoor2WorldCoor(image_coor, world_coor);
		ImgCoor2WorldCoor2(image_coor, world_coor);
		printf("%d\t%d\t%f\t%f\n",tracker_result.x + tracker_result.width / 2, tracker_result.y + tracker_result.height / 2, world_coor.x, world_coor.y);
        //       output_video << show_frame;

		float dis;
		c = (char)waitKey(10);

        if(27 == c)
        {
            return -1;
        }
        else if(c == 32)
        {
        	show_frame2 = show_frame.clone();
        	while(1)//(waitKey(10) != 32)
        	{
				char ch = waitKey(10);

                if(ch == 32){
                    break;
                }
                else if(ch == 'q'){
                    quit_flag = true;
                }
                else if(ch == 'r'){
                    record_flag = ~record_flag;
                }
				else if(ch == 'd'){
                    cout<< "start"<<endl;
					target1 = world_coor;
                    frame_count1 = frame_count;
					cout<<"current pos: "<<target1.x<< ", " <<target1.y <<"current frame: "<<frame_count1<<endl;
			    }
                else if(ch == 'f'){
					target2 = world_coor;
                    frame_count2 = frame_count;
					dis = pow(abs(target1.x-target2.x),2) +  pow(abs(target1.y-target2.y),2);
					dis = sqrt(dis);
//					dis = abs(target1.x-target2.x) +  abs(target1.y-target2.y);
					cout<<"current pos: "<<target2.x<<", "<< target2.y<<", dis: "<<dis<<", frame diff: "<<frame_count2-frame_count1<<endl;
				}
        		imshow("camera", show_frame2);
        	}
        }
        if(quit_flag)
            break;
    }
    if(output_video.isOpened())
        output_video.release();
    fileVideo.release();
    return 0;
}


